/**
 * Arduino两轮小车教程
 * 
 * car0801: 
 * 
 * site: http://www.xpstem.com/
 * author: billy zhang
 */
#include <Arduino.h>
#include "InfraredTrackingVehicle.h"

#define MaxPower 255
#define MinPower 120

#define LineTurnDelay 100

void InfraredTrackingVehicle::setup()
{
  
}

void InfraredTrackingVehicle::drive()
{
  // 找线
  int leftVal = analogRead(leftTracking_);
  int rightVal = analogRead(rightTracking_);
  leftVal = leftVal > 512 ? 1 : 0;
  rightVal = rightVal > 512 ? 1 : 0;
  int state = leftVal << 1 + rightVal;
  Serial.println(state);
  
  if (state==0 || state==3) {
    driven_.forward(MaxPower);// 11, 00 向前
    delay(300);
  } else {
    if (state==1) { // 01 偏右
      driven_.turnLeft(MinPower);
    } else if (state==2) { // 10 偏左
      driven_.turnRight(MinPower);
    }
    
    delay(LineTurnDelay);
  }
  driven_.stop();
  delay(300);
}

InfraredTrackingVehicle::~InfraredTrackingVehicle()
{
}
